load 'training.mat';

sigNum = 10; % the number of signature from one place
areaNum = 4; % classified area number
trainingSetSize = 19; % for each area, how many readings are taken as training data

% Set serial port
serialPortId = '/dev/tty.usbserial-FTFO9HPY';
serialPort = serial(serialPortId, 'BaudRate', 9600, 'DataBits', 8, 'Timeout', 1.5);

% Open Serial Port
try
    fopen(serialPort);
catch ME
    error('failed to open serial port');
end

while 1
    % Get command
    cmd = input('get localization? 1: yes; 0: quit');
    if cmd == 0
        break;
    end
    
    % Init
    close all; clc;
    load 'training.mat';
    signatures = []; % sigNum's signature
    rxInfo = []; % serial port msg
    i = 0; % counter for sigNum
    area = [0,0,0,0]; % collect area selected times

    while i < sigNum
        
        rxInfo = [rxInfo fscanf(serialPort)];

        if sum(ismember(rxInfo,',')) == 6
            
            info = textscan(rxInfo, '%d,%d,%d,%d,%d,%d,%d');
            rxInfo = [];
            fprintf('round %d, info: %d,%d,%d,%d,%d\r\n'...
                ,i+1,info{3},info{4},info{5},info{6},info{7});
            if info{1} == 2 & info{2} == 2 
                
                if sum(ismember([info{3},info{4},info{5},info{6},info{7}], -1)) == 0
                    
                    signatures = [signatures; ...
                        info{3},info{4},info{5},info{6},info{7}]; 
                    i = i + 1;
                    minDist = 10000;
                    suggestArea = -1;
                    
                    % Comparing fingerprints
                    for k = 1 : areaNum
                        for j = 1 : trainingSetSize
                            % Get distance between two signatures
                            tmp = [trainingSet{k}(j, :); signatures(i, :)];
                            temp = pdist(tmp, 'correlation'); % correlation / cosine / hamming / euclidean
                            % Record selected area
                            if temp < minDist
                                minDist = temp;
                                suggestArea = k;
                            end
                        end
                    end
                    % Add selected area's confidence
                    area(suggestArea) = area(suggestArea) + 1;
                end
            end
            
        elseif sum(ismember(rxInfo,',')) > 6
            flushinput(serialPort);
            flushoutput(serialPort);
            rxInfo = []; 
        end
        
    end
    % Visualize 
    area = area/sigNum;
    drawGrids(area);

end

% Close port
try
    flushinput(serialPort);
    flushoutput(serialPort);
    stopasync(serialPort);
    fclose(serialPort);
    delete(serialPort);
    clear serialPort;
catch ME
    error('fail to close the serial port, check connection and name'); 
end